package path;
import java.util.*;

class Node {
    int x, y; // 节点的坐标
    double g; // 从起始节点到当前节点的实际成本
    double h; // 从当前节点到目标节点的估算成本
    double f; // 总成本 f(n) = g(n) + h(n)
    Node parent; // 父节点，用于路径重构

    public Node(int x, int y) {
        this.x = x;
        this.y = y;
    }
}

public class AStarAlgorithm {
    public static final int[][] GRID = {
            {0, 0, 0, 0, 1, 0},
            {0, 1, 0, 0, 0, 0},
            {0, 0, 1, 1, 0, 0},
            {0, 0, 0, 1, 1, 0},
            {1, 0, 0, 0, 0, 0},
            {0, 0, 0, 0, 0, 0}
    };
    // 标记为没访问过，通过坐标进行判断，如果通过节点的话得重写很多方法
    public static boolean[][] visited = new boolean[GRID.length][GRID[0].length];
    // 标记为已访问过，通过坐标进行判断，如果通过节点的话得重写很多方法
    public static boolean[][] unVisited = new boolean[GRID.length][GRID[0].length];

    public static void main(String[] args) {
        Node start = new Node(0, 0);
        Node end = new Node(5, 5);
        List<Node> path = findPath(start, end);

        if (path != null) {
            System.out.println("最短路径：");
            for (Node node : path) {
                System.out.println("(" + node.x + ", " + node.y + ")");
            }
        } else {
            System.out.println("未找到路径！");
        }
    }

    /**
     * Java中对象直接比较是看hash值的，判断位置是否访问过还是得通过坐标进行比较
     * 是否到达终点也需要通过坐标比较，通过对象比较不靠谱
     */
    public static List<Node> findPath(Node start, Node end) {
        // 未访问过的节点，使用小顶堆保证每次优先取出综合距离最短的
        PriorityQueue<Node> openSet = new PriorityQueue<>(Comparator.comparingDouble(node -> node.f));
        openSet.add(start);
        unVisited[start.x][start.y] = true;
        while (!openSet.isEmpty()) {
            Node current = openSet.poll();
            unVisited[current.x][current.y] = false;
            if (current.x == end.x && current.y == end.y) {// 到达了终点，直接构建路径
                return reconstructPath(current);
            }
            visited[current.x][current.y] = true;// 标记为已访问
            for (Node neighbor : getNeighbors(current)) {//TODO 这里也可以优化搜索方式
                double tentativeG = current.g + 1; //TODO 假设每个相邻节点的实际成本都为1，这里是可以优化的
                if (!unVisited[neighbor.x][neighbor.y] || tentativeG < neighbor.g) {
                    neighbor.parent = current;
                    neighbor.g = tentativeG;
                    neighbor.h = calculateHeuristic(neighbor, end);//TODO 这里用的算法也可以优化
                    neighbor.f = neighbor.g + neighbor.h;
                    if (!unVisited[neighbor.x][neighbor.y]) {
                        openSet.add(neighbor);
                        unVisited[neighbor.x][neighbor.y] = true;
                    }
                }
            }
        }

        return null; // 未找到路径
    }

    /**
     * 从不同方向寻找邻居节点，有将8方向优化成5方向的
     * 根据初始点与终点的夹角方向进行相邻节点的舍去
     */
    public static List<Node> getNeighbors(Node node) {
        List<Node> neighbors = new ArrayList<>();
        int x = node.x;
        int y = node.y;
        // int[][] directions = {{-1, 0}, {1, 0}, {0, -1}, {0, 1}, {1, 1}, {-1, -1}, {-1, 1}, {1, -1}};
        int[][] directions = {{-1, 0}, {1, 0}, {0, -1}, {0, 1}};

        for (int[] dir : directions) {
            int newX = x + dir[0];
            int newY = y + dir[1];
            if (isValid(newX, newY)) {
                neighbors.add(new Node(newX, newY));
            }
        }

        return neighbors;
    }

    public static boolean isValid(int x, int y) {
        return x >= 0 && x < GRID.length && y >= 0 && y < GRID[0].length && GRID[x][y] == 0 && !visited[x][y];
    }

    /**
     * 启发函数，这里对搜索结果影响很大，可以添加权重系数对H值进行优化（动态加权）
     * 曼哈顿距离：只能上下左右走，计算效率很高
     * 欧几里得距离：不限维度，不限走法
     * 切比雪夫距离：求对角线距离
     */
    public static double calculateHeuristic(Node node, Node end) {
        // return (int) Math.sqrt(Math.pow(node.x - end.x, 2) + Math.pow(node.y - end.y, 2));
        double weight = 1;
        double d = Math.abs(node.x - end.x) + Math.abs(node.y - end.y); // 曼哈顿距离
        if (d >= 18) {
            weight = 3.0;
        }else {
            weight = 0.8;
        }
        return weight * d;
    }

    public static List<Node> reconstructPath(Node node) {
        List<Node> path = new ArrayList<>();
        while (node != null) {
            path.add(node);
            node = node.parent;
        }
        Collections.reverse(path);
        return path;
    }
}



